/**
 * @file findline.c
 * @author sevenfite(sevenfite@163.com)
 * @brief 巡线方法
 * @version 0.1
 * @date 2024-01-09
 *
 * @copyright Copyright (c) 2024
 *
 *
 */

#include "findline.h"
#include "control.h"
#include "motor.h"
#include "pid.h"
#include "C_code_Log.h"
PIDTypeDef gl_sensorPID;  // 传感器PID

extern int getSensorDigitalRank();  // 传感器分级/待实现

int getSensorDigitalRank() { return 0; }
/*

* 函数介绍：缺省巡线方法
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的P I
D三个参数
* 作者    ：@断忆

*/
static void flMethod_default() {
  LOG("INFO", "flMethod_default");
  return;
  int rank;
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    PID_Init(&gl_sensorPID, 20, 3000, 250, 0,
             100);  // 对速度PID的参数进行初始化设置
    gl_controlCenter.findLineFlag = 1;
  }
  rank = getSensorDigitalRank();                    // 分级
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(3000, 3000, difSpeed);  // 位置式改变电机速度
}
/*

* 函数介绍：过桥巡线方法
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的P I
D三个参数
* 作者    ：@断忆

*/
static void flMethod_bridge() {
  LOG("INFO", "flMethod_bridge");
  return;
  int rank;
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    PID_Init(&gl_sensorPID, 16, 3000, 350, 0,
             0);  // 对速度PID的参数进行初始化设置
    gl_controlCenter.findLineFlag = 1;
  }
  rank = getSensorDigitalRank();                    // 分级
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(3000, 3000, difSpeed);  // 位置式改变电机速度
}

/// @brief 巡线任务
/// @param control 控制台
/// @note 通过控制台的findLineWay来选择使用哪个巡线方法
/// @note 这个巡线方法是有可能在加速过程或者过障碍中被修改的
void findLine_Task(const ControlCenterTypeDef *control) {
  // 如果巡线任务状态为准备就绪
  if (control->runState->F_LineState == MY_READY) {
    // 根据巡线任务的类型执行不同的巡线任务
    switch (control->findLineWay) {
      case FL_default:
        flMethod_default();
        break;
      case FL_bridge:
        flMethod_bridge();
        break;
      default:
        break;
    }
  }
}